#include "nvm.h"
#include "utils/crc16.h"
#include "bsp/cpu_driver.h"
#include "utils/logger.h"

#define NVM_MAGIC 0x12349876

typedef struct {
	u32  crc32;
	u32  len;
}piece_head_t;

typedef struct {
	u32  length;
	u32  address;
	bool erased;
	piece_head_t *piece;
}piece_ll_t;

typedef struct {
	u32 magic;
	u32 index;
	u32 crc32;
}nvm_head_t;

/* nvm flash layout 
|------------------|
    nvm_head_t
|------------------|
    piece_head_t 1
|------------------|
      data 1
|------------------|
    piece_head_t 2
|------------------|
      data 2
|------------------|
    piece_head_t 3
|------------------|
      data 3
|------------------|
       ...
       ...
*/

static piece_ll_t _nvm_piece[2][CONFIG_NVM_PICES_NUM];
static int _read_block = 0;

static u32 _pices_crc32(piece_head_t *p) {
	u32 address = (u32)&p->len;
	int len = p->len + 4;
	return crc32_get((u8 *)address, len);
}

static piece_ll_t *_piece(int b, int p) {
	return &_nvm_piece[b][p];
}

static u32 _nvm_start(int b) {
	return ((u32)_piece(b, 0)->piece - sizeof(nvm_head_t));
}

static bool _check_writeable(u32 addr, int size) {
	u32 *buff = (u32 *)addr;
	for (int i = 0; i < size/4; i++ ) {
		if (buff[i] != 0xFFFFFFFF) {
			return false;
		}
	}
	return true;
}

static void _erase_one_block(int b, u32 index) {
	u32 address = _nvm_start(b);
	sys_debug("erase block %d, addr 0x%x\n", b, address);
	nvm_head_t head;
	head.magic = NVM_MAGIC;
	head.index = index;
	head.crc32 = crc32_get((u8 *)&head, 8);
	flash_write_page(address, (u8 *)&head, sizeof(head), true);
	for (int i = 0; i < CONFIG_NVM_PICES_NUM; i++) {
		_piece(b, i)->erased = true;
	}
	sys_debug("erase OK\n");
}

static int _block_index(int b) {
	nvm_head_t *head = (nvm_head_t *)_nvm_start(b);
	return head->index;
}

static int _check_one_block(int b, int *index) {
	nvm_head_t *head = (nvm_head_t *) _nvm_start(b);
	if (head->magic != NVM_MAGIC || head->index == 0xFFFFFFFF || head->crc32 != crc32_get((u8 *)head, 8)) {
		sys_debug("block%d is not inited\n", b);
		return -1; //head error, need erase
	}
	*index = head->index;
	int w_n = 0;
	for (int n = 0; n < CONFIG_NVM_PICES_NUM; n++) {
		piece_head_t *flash_pice = _piece(b, n)->piece;
		if (flash_pice->len <= _piece(b, n)->length && flash_pice->crc32 == _pices_crc32(flash_pice)) {
			w_n++;
			_piece(b, n)->erased = false;
		}else {
			if (_check_writeable((u32)flash_pice, _piece(b, n)->length + sizeof(piece_head_t))) {
				_piece(b, n)->erased = true;
			}else {
				_piece(b, n)->erased = false;
			}
		}
		sys_debug("block%d pices %d is %s\n", b, n, _piece(b, n)->erased?"Erased":"NoErased");
	}
	return w_n;
}

void nvm_init(u32 flash_addr[], u32 sizes[]) {
	for (int b = 0; b < 2; b++) {
		u32 address = flash_addr[b] + sizeof(nvm_head_t);// + sizeof(piece_head_t) * CONFIG_NVM_PICES_NUM;
		u32 f_size = sizeof(nvm_head_t);
		for (int n = 0; n < CONFIG_NVM_PICES_NUM; n++) {
			_nvm_piece[b][n].length = sizes[n];
			_nvm_piece[b][n].piece = (piece_head_t *)address;
			_nvm_piece[b][n].address = address + sizeof(piece_head_t);
			_nvm_piece[b][n].erased = false;
			//sys_debug("NVM Init: block%d piece:%d, flash head 0x%x, data 0x%x\n", b, n, _nvm_piece[b][n].piece, _nvm_piece[b][n].address);
			address += (sizes[n] + sizeof(piece_head_t));
			f_size += (sizes[n] + sizeof(piece_head_t));
		}
		sys_debug("NVM: total size %d\n", f_size);
	}
	int index0 = -1, index1 = -1;
	int b0 = _check_one_block(0, &index0);
	int b1 = _check_one_block(1, &index1);
	sys_debug("NVM: %d, %d, %d, %d\n", index0, b0, index1, b1);
	if (b0 < 0) {
		_erase_one_block(0, index1 + 1);
	}
	if (b1 < 0) {
		_erase_one_block(1, index0 + 1);
	}

	if(b1 > b0){
		_read_block = 1;	
	}else if (b1 < b0) {
		_read_block = 0;
	}else {
		if (index1 > index0) {
			_read_block = 1;
		}else {
			_read_block = 0;
		}
	}
	sys_debug("NVM: read block %d\n", _read_block);
}

s32 nvm_read(u32 pices_id, u8 *buffer, u32 len) {
	sys_debug("NVM: read pice %d, len = %d\n", pices_id, len);
	if (pices_id >= CONFIG_NVM_PICES_NUM || len > _piece(_read_block, pices_id)->length) {
		
		return NVM_PARAM_ERR;
	}
	u32 data_addr = _piece(_read_block, pices_id)->address;
	piece_head_t *flash_pice = _piece(_read_block, pices_id)->piece;
	sys_debug("NVM read block%d, piece:%d, addr 0x%x, flash addr 0x%x\n", _read_block, pices_id, data_addr, flash_pice);
	if (flash_pice->len != len) {
		sys_debug("Len err %d, %d\n", len, flash_pice->len);
		return NVM_ERR_LEN;
	}
	if (flash_pice->crc32 != _pices_crc32(flash_pice)) {
		sys_debug("crc error\n");
		return NVM_ERR_CRC;
	}
	memcpy(buffer, (void *)data_addr, len);
	
	return NVM_ERR_NO;
}

void _write_one_pices(u32 target_block, u32 pices_id, u8 *buffer, u32 len) {
	u32 data_addr = _piece(target_block, pices_id)->address;
	sys_debug("NVM: write block%d, piece:%d, address 0x%x\n", target_block, pices_id, data_addr);
	if (!_check_writeable((u32)_piece(target_block, pices_id)->piece, _piece(target_block, pices_id)->length)) {
		sys_debug("can not write\n");
		return;
	}
	flash_write_page(data_addr, buffer, len, false);
	data_addr = data_addr - 4;
	sys_debug("write piece address 0x%x\n", data_addr);
	flash_write_page(data_addr, (u8 *)&len, 4, false);
	u32 crc32 = _pices_crc32(_piece(target_block, pices_id)->piece);
	data_addr = data_addr - 4;
	sys_debug("write piece address 0x%x\n", data_addr);
	flash_write_page(data_addr, (u8 *)&crc32, 4, false);
	 _piece(target_block, pices_id)->erased = false;
}


void _copy_nvm_skip_pice(int src_b, int dst_b, u32 skip) {
	for (int i = 0; i < CONFIG_NVM_PICES_NUM; i++) {
		if (i == skip) {
			continue;
		}
		u32 src = _piece(src_b, i)->address - sizeof(piece_head_t);
		u32 dst = _piece(dst_b, i)->address - sizeof(piece_head_t);
		int len = _piece(src_b, i)->length;
		flash_write_page(dst, (u8 *)src, len, false);
		_piece(dst_b, i)->erased = _piece(src_b, i)->erased;
	}
}

s32 nvm_write(u32 pices_id, u8 *buffer, u32 len) {
	sys_debug("NVM: write pice %d, len = %d\n", pices_id, len);
	int target_block = 1 - _read_block;
	if (pices_id >= CONFIG_NVM_PICES_NUM || len > _piece(_read_block, pices_id)->length) {
		return NVM_PARAM_ERR;
	}
	sys_debug("NVM: write block%d, piece: %d\n", target_block, pices_id);
	if (!_piece(target_block, pices_id)->erased) {
		sys_debug("need erase the block\n");
		_erase_one_block(target_block, _block_index(_read_block) + 1);
		_copy_nvm_skip_pice(_read_block, target_block, pices_id);
	}
	_write_one_pices(target_block, pices_id, buffer, len);
	_read_block = 1 - _read_block;
	return NVM_ERR_NO;
}


void nvm_test(void) {
	u32 addr[] = {CONFIG_NVM0_ADDRESS, CONFIG_NVM1_ADDRESS};
	u32 pieces[] = {16*1024, 4096, 8192, 8192};
	nvm_init(addr, pieces);

	u8 buffer[32];

	sys_debug("result1 = %d\n", nvm_read(0, buffer, 32));
	for (int i = 0; i < sizeof(buffer); i++) {
		buffer[i] = (u8)i;
	}
	nvm_write(0, buffer, 32);

	nvm_write(3, buffer, 32);

	int result = nvm_read(0, buffer, 32);
	
	sys_debug("result2 = %d\n", result);
	if (result == NVM_ERR_NO) {
		for (int i = 0; i < 32; i++) {
			sys_debug("%d ", buffer[i]);
			if (i % 16 == 0) {
				sys_debug(" \n");
			}
		}
	}
}
